
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mc_circle.h
  * @author     baiyang
  * @date       2022-10-10
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <ahrs/ahrs_view.h>
#include <common/gp_defines.h>
#include <common/time/gp_time.h>
#include <common/location/location.h>
#include <common/gp_math/gp_mathlib.h>
#include <mc_position_control/mc_position_control.h>
/*-----------------------------------macro------------------------------------*/
// loiter maximum velocities and accelerations
#define MB_CIRCLE_RADIUS_DEFAULT    1000.0f     // radius of the circle in cm that the vehicle will fly
#define MB_CIRCLE_RATE_DEFAULT      20.0f       // turn rate in deg/sec.  Positive to turn clockwise, negative for counter clockwise
#define MB_CIRCLE_ANGULAR_ACCEL_MIN 2.0f        // angular acceleration should never be less than 2deg/sec
#define MB_CIRCLE_RADIUS_MAX        200000.0f   // maximum allowed circle radius of 2km
/*----------------------------------typedef-----------------------------------*/
// get expected source of terrain data
enum CircleTerrainSource {
    CIRCLE_TERRAIN_UNAVAILABLE,
    CIRCLE_TERRAIN_FROM_RANGEFINDER,
    CIRCLE_TERRAIN_FROM_TERRAINDATABASE,
};

enum CircleOptions {
    CIRCLE_OPTIONS_MANUAL_CONTROL           = 1U << 0,
    CIRCLE_OPTIONS_FACE_DIRECTION_OF_TRAVEL = 1U << 1,
    CIRCLE_OPTIONS_INIT_AT_CENTER           = 1U << 2, // true then the circle center will be the current location, false and the center will be 1 radius ahead
};

typedef struct mc_circle* mc_circle_t;

struct mc_circle {
    // flags structure
    struct circle_flags {
        uint8_t panorama    : 1;    // true if we are doing a panorama
    } _flags;

    const ahrs_view*     _ahrs;
    Position_ctrl* _pos_control;

    // parameters
    Param_float    _radius_parm;   // radius of circle in cm loaded from params
    Param_float    _rate;          // rotation speed in deg/sec
    Param_int16    _options;       // stick control enable/disable

    // internal variables
    Vector3p    _center;        // center of circle in cm from home
    float       _radius;        // radius of circle in cm
    float       _yaw;           // yaw heading (normally towards circle center)
    float       _angle;         // current angular position around circle in radians (0=directly north of the center of the circle)
    float       _angle_total;   // total angle traveled in radians
    float       _angular_vel;   // angular velocity in radians/sec
    float       _angular_vel_max;   // maximum velocity in radians/sec
    float       _angular_accel; // angular acceleration in radians/sec/sec
    uint32_t    _last_update_ms;    // system time of last update
    float       _last_radius_param; // last value of radius param, used to update radius on param change

    // terrain following variables
    bool        _terrain_alt;           // true if _center.z is alt-above-terrain, false if alt-above-ekf-origin
    bool        _rangefinder_available; // true if range finder could be used
    bool        _rangefinder_healthy;   // true if range finder is healthy
    float       _rangefinder_terrain_offset_cm; // latest rangefinder based terrain offset (e.g. terrain's height above EKF origin)
};

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
/// Constructor
void circlenav_ctor(mc_circle_t circle, const ahrs_view* ahrs, Position_ctrl* pos_control);

/// init - initialise circle controller setting center specifically
///     set terrain_alt to true if center.z should be interpreted as an alt-above-terrain
///     caller should set the position controller's x,y and z speeds and accelerations before calling this
void circlenav_init(mc_circle_t circle, const Vector3p* center, bool terrain_alt);

/// init - initialise circle controller setting center using stopping point and projecting out based on the copter's heading
///     caller should set the position controller's x,y and z speeds and accelerations before calling this
void circlenav_init2(mc_circle_t circle);

/// set circle center to a Location
void circlenav_set_center(mc_circle_t circle, const Location* center);

/// set_circle_center as a vector from ekf origin
///     terrain_alt should be true if center.z is alt is above terrain
static inline void circlenav_set_center2(mc_circle_t circle, const Vector3f_t* center, bool terrain_alt) { circle->_center = *center; circle->_terrain_alt = terrain_alt; }

/// get_circle_center in cm from home
static inline const Vector3p* circlenav_get_center(mc_circle_t circle) { return &circle->_center; }

/// set_circle_rate - set circle rate in degrees per second
void circlenav_set_rate(mc_circle_t circle, float deg_per_sec);

/// set_circle_rate - set circle rate in degrees per second
void circlenav_set_radius_cm(mc_circle_t circle, float radius_cm);

/// returns true if update has been run recently
/// used by vehicle code to determine if get_yaw() is valid
bool circlenav_is_active(mc_circle_t circle);

/// update - update circle controller
bool circlenav_update(mc_circle_t circle, float climb_rate_cms);

// get_closest_point_on_circle - returns closest point on the circle
//  circle's center should already have been set
//  closest point on the circle will be placed in result
//  result's altitude (i.e. z) will be set to the circle_center's altitude
//  if vehicle is at the center of the circle, the edge directly behind vehicle will be returned
void circlenav_get_closest_point_on_circle(mc_circle_t circle, Vector3f_t *result);

void circlenav_check_param_change(mc_circle_t circle);

/// returns true if using terrain altitudes
static inline bool circlenav_center_is_terrain_alt(mc_circle_t circle) { return circle->_terrain_alt; }

/// get_radius - returns radius of circle in cm
static inline float circlenav_get_radius(mc_circle_t circle) { return math_flt_positive(circle->_radius)?circle->_radius:circle->_radius_parm; }

/// get_rate - returns target rate in deg/sec held in RATE parameter
static inline float circlenav_get_rate(mc_circle_t circle) { return circle->_rate; }

/// get_rate_current - returns actual calculated rate target in deg/sec, which may be less than _rate
static inline float circlenav_get_rate_current(mc_circle_t circle) { return ToDeg(circle->_angular_vel); }

/// get_angle_total - return total angle in radians that vehicle has circled
static inline float circlenav_get_angle_total(mc_circle_t circle) { return circle->_angle_total; }

/// get desired roll, pitch which should be fed into stabilize controllers
static inline float circlenav_get_roll(mc_circle_t circle) { return posctrl_get_roll_cd(circle->_pos_control); }
static inline float circlenav_get_pitch(mc_circle_t circle) { return posctrl_get_pitch_cd(circle->_pos_control); }
static inline Vector3f_t circlenav_get_thrust_vector(mc_circle_t circle) { return posctrl_get_thrust_vector(circle->_pos_control); }
static inline float circlenav_get_yaw(mc_circle_t circle) { return circle->_yaw; }

/// get horizontal distance to loiter target in cm
static inline float circlenav_get_distance_to_target(mc_circle_t circle) { return posctrl_get_pos_error_xy_cm(circle->_pos_control); }

/// get bearing to target in centi-degrees
static inline int32_t circlenav_get_bearing_to_target(mc_circle_t circle) { return posctrl_get_bearing_to_target_cd(circle->_pos_control); }

/// true if pilot control of radius and turn rate is enabled
static inline bool circlenav_pilot_control_enabled(mc_circle_t circle) { return (circle->_options & CIRCLE_OPTIONS_MANUAL_CONTROL) != 0; }

/// provide rangefinder based terrain offset
/// terrain offset is the terrain's height above the EKF origin
static inline void circlenav_set_rangefinder_terrain_offset(mc_circle_t circle, bool use, bool healthy, float terrain_offset_cm) { circle->_rangefinder_available = use; circle->_rangefinder_healthy = healthy; circle->_rangefinder_terrain_offset_cm = terrain_offset_cm;}
/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



